import com.pi4j.wiringpi.Gpio;
import com.raspoid.Tools;
import com.raspoid.additionalcomponents.PCA9685;
import com.raspoid.additionalcomponents.PCA9685.PCA9685Channel;
import com.raspoid.additionalcomponents.servomotor.MicroServo9gA0090;
import com.raspoid.additionalcomponents.servomotor.ServoMotor;
import com.raspoid.additionalcomponents.servomotor.TowerProMG90S;


public class servo {
	public static void main(String[] args) {
		// Comment/Uncomment the corresponding sections to use PCA9685
        // or hardware PWM pins from the Raspberry Pi
        ServoMotor motor;
        ServoMotor motor2;
        ServoMotor motor3;
        ServoMotor motor4;
        Gpio.wiringPiSetup();
        // Using a PWM pin
//        motor = new TowerProMG90S(PWMPin.PWM0); // NOSONAR
        
        Tools.log("init ");
        // Using a PCA9685
        motor3 = new TowerProMG90S(new PCA9685(), PCA9685Channel.CHANNEL_02); // NOSONAR
//        motor4 = new MicroServo9gA0090(new PCA9685(), PCA9685Channel.CHANNEL_03); // NOSONAR
//        motor = new MicroServo9gA0090(new PCA9685(), PCA9685Channel.CHANNEL_00);
//        motor2 = new MicroServo9gA0090(new PCA9685(), PCA9685Channel.CHANNEL_01);
//        double[] angles = {0, 35, 70, 105, 140};
        double[] angles = {0,45,90,135,180};
        motor3.setPWMFreq(57);
        motor3.setAngle(90);
        System.out.println(motor3.getPositionAngle());
        Tools.sleepMilliseconds(1000);
//        
//        motor3.setAngle(65);
//        System.out.println(motor3.getPositionAngle());
//        Tools.sleepMilliseconds(1000);
//        
//        motor3.setAngle(35);
//        System.out.println(motor3.getPositionAngle());
//        Tools.sleepMilliseconds(1000); 
//        motor3.setAngle(90);
//        System.out.println(motor3.getPositionAngle());
//        Tools.sleepMilliseconds(1000);
//        
//        motor3.setAngle(115);
//        System.out.println(motor3.getPositionAngle());
//        Tools.sleepMilliseconds(1000);
        
        for (int i = 0; i < angles.length; i++) {
        	 motor3.setAngle(angles[i]);
             System.out.println(motor3.getPositionAngle());
             Tools.sleepMilliseconds(1000);
		}
       
//        
//        motor3.setAngle(180);
//        System.out.println(motor3.getPositionAngle());
//        Tools.sleepMilliseconds(1000);
	}
	
}
